//
// Created by daybeha on 23-11-27.
//
#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <boost/bind.hpp>

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>
#include "tf2/LinearMath/Quaternion.h"

using namespace std;


nav_msgs::Path path;

ros::Publisher pub_odom, pub_path;
// 初始化位姿
Eigen::Vector3d Pwb = Eigen::Vector3d::Zero();
Eigen::Quaterniond Qwb = Eigen::Quaterniond::Identity();
Eigen::Vector3d Vw = Eigen::Vector3d::Zero();   // velocity in world space
Eigen::Vector3d gw = Eigen::Vector3d(0, 0, 9.82);          //  重力加速度(躺放)

bool have_orientation;
double pre_time;
Eigen::Vector3d pre_acc;
Eigen::Vector3d pre_gyro;

string traj_outpath;


void add_pose(nav_msgs::Odometry &odom_msg, geometry_msgs::TransformStamped &tf_msg){
    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.header.stamp = ros::Time();
    pose_stamped.header.frame_id = "odom";

//        Pwb = Pwb/resize_factor;
    pose_stamped.pose.position.x = Pwb(0);
    pose_stamped.pose.position.y = Pwb(1);
    pose_stamped.pose.position.z = Pwb(2);
    tf_msg.transform.translation.x = Pwb(0);
    tf_msg.transform.translation.y = Pwb(1);
    tf_msg.transform.translation.z = Pwb(2);
    odom_msg.pose.pose.position = pose_stamped.pose.position;

    pose_stamped.pose.orientation.w = Qwb.w();
    pose_stamped.pose.orientation.x = Qwb.x();
    pose_stamped.pose.orientation.y = Qwb.y();
    pose_stamped.pose.orientation.z = Qwb.z();
    odom_msg.pose.pose.orientation = pose_stamped.pose.orientation;
    tf_msg.transform.rotation = pose_stamped.pose.orientation;

    path.poses.push_back(pose_stamped);
}

void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg,  tf::TransformBroadcaster &tf_br)
{
    // 处理IMU数据，进行必要的计算
    cout << "acc(gw): " << imu_msg->linear_acceleration.x << " " << imu_msg->linear_acceleration.y << " " << imu_msg->linear_acceleration.z << endl;
//    cout << "gyro: " << msg->angular_velocity.x << " " << msg->angular_velocity.y << " " << msg->angular_velocity.z << endl;
//    cout << "orientation: " << msg->orientation.w << " " << msg->orientation.x << " " << msg->orientation.y << " " << msg->orientation.z << endl;


    //ROS2中的时间类型builtin_interfaces::msg::Time是需要把秒和纳秒组合起来才能表示当前时间的
    double curr_time = imu_msg->header.stamp.toSec();
    Eigen::Vector3d curr_acc(imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z);
    Eigen::Vector3d curr_gyro(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z);
    if (have_orientation){
        Qwb = Eigen::Quaterniond(imu_msg->orientation.w, imu_msg->orientation.x, imu_msg->orientation.y, imu_msg->orientation.z);
    }


    Eigen::Vector3d dtheta_half, acc_w;
    if(pre_time!=0){
        double dt = curr_time - pre_time;
        Eigen::Vector3d gyro_w = Qwb * (curr_gyro + pre_gyro)/2.0;
        acc_w = Qwb * (curr_acc + pre_acc)/ 2.0 - gw;  // aw = Pwb * ( acc_body - acc_bias ) - gw
        cout << "acc: " << acc_w(0) << " " << acc_w(1) << " " << acc_w(2) << endl;

        // 路径积分
        Pwb = Pwb + Vw * dt + 0.5 * acc_w * dt * dt ;
        Vw = Vw + acc_w * dt;
        if(!have_orientation){
            dtheta_half =  (curr_gyro + pre_gyro) * dt /2.0 ;
            Qwb = Qwb * Eigen::Quaterniond(1, dtheta_half[0]/2,
                                           dtheta_half[1]/2, dtheta_half[2]/2).normalized();
        }
        cout << "Qwb: " << Qwb.w() << " " << Qwb.x() << " " << Qwb.y() << " " << Qwb.z() << endl;
        cout << "Pwb: " << Pwb(0) << " " << Pwb(1) << " " << Pwb(2) << endl;


        /// 转为ros消息
        // 创建Odometry消息
        nav_msgs::Odometry odom_msg;
        odom_msg.header.stamp = imu_msg->header.stamp;
        odom_msg.header.frame_id = "odom";
        odom_msg.child_frame_id = "base_link";
        // 创建TF消息
        geometry_msgs::TransformStamped tf_msg;
        tf_msg.header = odom_msg.header;
        tf_msg.child_frame_id = odom_msg.child_frame_id;

        // 填充Twist信息
        odom_msg.twist.twist.linear.x = acc_w(0);
        odom_msg.twist.twist.linear.y = acc_w(1);
        odom_msg.twist.twist.linear.z = acc_w(2);
        odom_msg.twist.twist.angular.x = gyro_w(0);
        odom_msg.twist.twist.angular.y = gyro_w(1);
        odom_msg.twist.twist.angular.z = gyro_w(2);

        // 填充Pose信息
        add_pose(odom_msg, tf_msg);

        // 发布Path Odometry TFs消息
        pub_path.publish(path);
        pub_odom.publish(odom_msg);
        tf_br.sendTransform(tf_msg);
    }

    pre_time = curr_time;
    pre_acc = curr_acc;
    pre_gyro = curr_gyro;
}



int main(int argc, char **argv) {
    ros::init(argc, argv, "imu2odom");
    ros::NodeHandle node("~");
//    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);

    int traj_format; // 0: kitti; 1:tum
    node.param<int>("/traj_format", traj_format, 1);
    node.param<string>("/traj_outpath", traj_outpath,
                       "/home/daybeha/Documents/github/vins_ws/src/VINS-Fusion/results/");

    string imu_topic;
    node.param<string>("/imu_topic", imu_topic,
                       "/imu/data_raw");


    pub_odom = node.advertise<nav_msgs::Odometry>("/imu_odom", 10);
    pub_path = node.advertise<nav_msgs::Path>("/imu_path", 10);
    path.header.stamp = ros::Time(0);
    path.header.frame_id="odom";

    tf::TransformBroadcaster tf_br;
    ros::Subscriber sub_imu=node.subscribe<sensor_msgs::Imu>(imu_topic, 2000, boost::bind(&imu_callback,_1,tf_br));

    ros::spin();

//    if (traj_format == 0)
//        save_trajectory_kitti();
//    else
//        save_trajectory_tum();


    return 0;
}
